#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/Imu.h>
#include <string>
#include <vector>

#include "com.h"

using namespace std;

ros::Publisher pub;

sensor_msgs::Imu gyro;

vector<vector<double> > angle_history;
int current_history = 0;
int history_size = 5; // più lunga più rallenta ma ci sono meno scatti
					  // da tenere giù durante il debugging

bool arms_mirroring = true;

// robot parts mapping
int right_knee_joint_pitch = 0,
	left_arm_joint_pitch = 1,
	left_hip_joint_pitch = 2,
	left_hip_joint_roll = 3,
	right_ankle_joint_pitch = 4,
	right_hip_joint_roll = 5,
	right_hip_joint_pitch = 6,
	left_shoulder_joint_pitch = 7,
	left_knee_joint_pitch = 8,
	head_joint_yaw = 9,
	left_shoulder_joint_roll = 10,
	right_shoulder_joint_pitch = 11,
	left_ankle_joint_pitch = 12,
	right_ankle_joint_roll = 13,
	right_arm_joint_pitch = 14,
	right_shoulder_joint_roll = 15,
	left_ankle_joint_roll = 16;

/*
    Print vector of double

    Params:
      const vector<double> & v
*/
void printVec(const vector<double> & v)
{
  for(int i = 0; i < v.size(); i++)
    cout << v[i] << "\t";
  cout << endl;
}

/*
    Sign function

    Params:
      const double d
*/
int sign(const double d)
{
	return d >= 0 ? 1 : -1;
}

/*
    Put a new entry in the history of the positions,
    only last history_size positions are stored in 
    a circular buffer

    Params:
      const vector<double> values: positions
*/
void storeInHistory(const vector<double> & values) {
	for(int i = 0; i < angle_history.size(); i++) {
		angle_history[i][current_history] = values[i];
	}
	current_history = (current_history+1)%history_size;
}

/*
    This function returns the mean position 
    considering the last history_size positions
*/
vector<double> getHistoryAvgs() {
	//cout << "Returning history avgs" << endl;
	vector<double> histAvgs;
	for(int i = 0; i<angle_history.size();i++) {
		double tmp_avg = 0;
		for(int j = 0; j<angle_history[i].size(); j++) {
			tmp_avg += angle_history[i][j];
		}

		//cout << i << " : ";
		//printVec(angle_history[i]);
		
		histAvgs.push_back(double(tmp_avg)/angle_history[i].size());
	}

	//printVec(histAvgs);
																																																																																																																																																																																																																																																																																																																																																																																																																																																																																			
	return histAvgs;
}

/*
	Symmetry assumption, i.e. mirror robot right side to left,
	it provides stability,
	arms mirroring is optional, it is useful for the grasping

	Params:
      vector<double> & positions, bool arms_mirroring = true
*/
void right2leftMirror(vector<double> & positions, bool arms_mirroring = true) {

	positions[ left_ankle_joint_roll ] = - positions[ right_ankle_joint_roll ];
	positions[ left_hip_joint_roll ] = - positions[ right_hip_joint_roll ];
	positions[ left_ankle_joint_pitch ] = positions[ right_ankle_joint_pitch ];
	positions[ left_knee_joint_pitch ] = positions[ right_knee_joint_pitch ];
	positions[ left_hip_joint_pitch ] = positions[ right_hip_joint_pitch ];

	// il mirror della braccia è utile quando il robot afferra l'oggetto
	if (arms_mirroring)
	{
	  positions[ left_shoulder_joint_roll ] = -positions[ right_shoulder_joint_roll ];
	  positions[ left_shoulder_joint_pitch ] = positions[ right_shoulder_joint_pitch ];
	  positions[ left_arm_joint_pitch ] = -positions[ right_arm_joint_pitch ];
	}
}

/*
	In both squat and bow models
	knee = -2 * hip (= -2 * ankle)

	Params:
      const double & hip_pitch
*/
double hip2knee(const double & hip_pitch) {

	return (- hip_pitch * 2.0);

}

/*
	In squat model
	hip = ankle

	Params:
      const double & hip_pitch
*/
double hip2ankle(const double & hip_pitch) {

	return hip_pitch;

}

/*
	In both squat and bow models
	ankle = -knee / 2

	Params:
      const double & knee_pitch
*/
double knee2ankle(const double & knee_pitch) {

	return (- knee_pitch * 2.0);

}

/*
	Estimate knee from hip

	Params:
      const vector<double> & positions
*/
double estimate_knee_pitch(const vector<double> & positions) {

  double knee_pitch;

  knee_pitch = hip2knee(positions[right_hip_joint_pitch]);

  return knee_pitch;
}

/*
	Estimate ankle from hip

	Params:
      const vector<double> & positions
*/
double estimate_ankle_pitch(const vector<double> & positions) {

	double ankle_pitch = 0.0;
	
	ankle_pitch = hip2ankle(positions[right_hip_joint_pitch]);

	return ankle_pitch;

}

/*
	keepBestLeg e divarica le gambe,
	il valore migliore è il maggiore in valore assoluto perchè:
	la kinect spesso non riesce a veder bene entrambe le gambe
	ad esempio perché sono coperte dalle braccia durante il grasping

	Params:
      const vector<double> & positions
*/
void keepBestLeg(vector<double> & positions) {

	positions[ right_ankle_joint_pitch ] = abs( positions[ left_ankle_joint_pitch ] ) > abs( positions[ right_ankle_joint_pitch ]) ? positions[ left_ankle_joint_pitch ] : positions[ right_ankle_joint_pitch ];
	positions[ right_knee_joint_pitch ] = abs( positions[ left_knee_joint_pitch ] ) > abs( positions[ right_knee_joint_pitch ] ) ? positions[ left_knee_joint_pitch ] : positions[ right_knee_joint_pitch ];
	positions[ right_hip_joint_pitch ] = abs( positions[ left_hip_joint_pitch ] ) > abs( positions[ right_hip_joint_pitch ] ) ? positions[ left_hip_joint_pitch ] : positions[ right_hip_joint_pitch ];
	
	positions[ right_hip_joint_roll ] = -0.2; // gambe divaricate per una maggiore stabilità
	positions[ right_ankle_joint_roll ] = 0.2; // così entrambi i piedi poggiano per terra quando le gambe sono divaricate
											   // e il centro di massa non è più sballato verso l'altro piede
}

/*
	keepBestArm con mirroring delle braccia facoltativo,
	il principio è lo stesso di keepBestLeg e giustificato dal fatto che
	la kinect vede intermittentemente le braccia avanti e indietro
	e non vede bene l'angolo dei gomiti soprattutto se un braccio è coperto dall'altro

	Params:
      vector<double> & positions, 
      bool arms_mirroring = true
*/
void keepBestArm(vector<double> & positions, bool arms_mirroring = true) {

	if (arms_mirroring)
	{
	  // prendo il braccio che si alza di più in avanti
	  // prendo il valore assoluto perché a seconda che siano positivi o negativi mi servirebbe max o min
	  // mi serve negativo per spostargli le braccia in avanti
	  positions[ right_shoulder_joint_pitch ] = -max(abs(positions[ right_shoulder_joint_pitch ]), abs(positions[ left_shoulder_joint_pitch ]));

	  // prendo il polso che si piega di più
	  // ci serve positivo
	  positions[ right_arm_joint_pitch ] = max(abs(positions[ right_arm_joint_pitch ]), abs(positions[ left_arm_joint_pitch ]));
	}
	else //(!arms_mirroring)
	{
	  positions[ right_shoulder_joint_pitch ] = -abs(positions[ right_shoulder_joint_pitch ]);
	  positions[ right_arm_joint_pitch ] = abs(positions[ right_arm_joint_pitch ]);
	  positions[ left_shoulder_joint_pitch ] = -abs(positions[ left_shoulder_joint_pitch ]);
	  positions[ left_arm_joint_pitch ] = -abs(positions[ left_arm_joint_pitch ]);
	}
}

/*
	removeCollisions evita situazioni critiche per il robot,
	blocca il polso quando il braccio è lungo il fianco,
	massima presa, se la presa è troppo forte le braccia richiedono
	troppa corrente, alle gambe ne arriva troppo poca e il robot
	si scoordina, non riesce a rialzarsi e cade

	Params:
      vector<double> & positions
*/
void removeCollisions(vector<double> & positions, bool arms_mirroring = true) {

	// un valore delle ginocchia < 0 è una posizione innaturale per un umano
	// è evidentemente un errore della kinect
	// non è bene che sia finito nell'history un valore del ginocchio negativo
	if (positions[right_knee_joint_pitch] < 0)
	{
	  positions[right_knee_joint_pitch] = 0;
	  positions[right_ankle_joint_pitch] = 0;
	  positions[right_hip_joint_pitch] = 0;
	}

	positions[ right_shoulder_joint_roll ] = -0.1; // braccia lungo il fianco = 0
												   // leggermente più lontano dal fianco per evitare collisioni
												   // in caso di movimenti improvvisi delle gambe
	
	// quando il braccio è lungo il fianco blocca il polso
	double shoulder_limit = -0.4;
	if( positions[ right_shoulder_joint_pitch ] > shoulder_limit )
	{
		positions[ right_arm_joint_pitch ] = 0.0;
		//cout << "sto bloccando il polso destro" << endl;
	}

	if (!arms_mirroring)
	{
		positions[ left_shoulder_joint_roll ] = - positions[ right_shoulder_joint_roll ];
		if( positions[ left_shoulder_joint_pitch ] > shoulder_limit )
		{
			positions[ left_arm_joint_pitch ] = 0.0;
			//cout << "sto bloccando il polso sinistro" << endl;
		}
	}

	// piegamenti massimi di hip e ginocchio, non sono utili e rendono il movimento meno generale
	// eviterebbero lo sbattere della coscia sul polpaccio
	/*double hip_limit = -1; //-1.2;
	if( positions[ right_hip_joint_pitch ] < hip_limit )
		positions[ right_hip_joint_pitch ] = hip_limit;

	double knee_limit = 2; //2.5;
	if( positions[ right_knee_joint_pitch ] > knee_limit )
	{
		positions[ right_knee_joint_pitch ] = knee_limit;
		cout << "sto bloccando il ginocchio perché l'ho piegato troppo" << endl;
	}*/

	// estremale per i polsi, influenza la presa, evita il contatto delle mani (il min è -1.57)
	double arm_limit = 0.85; // 0.8
	if( positions[ right_arm_joint_pitch ] > arm_limit )
		positions[ right_arm_joint_pitch ] = arm_limit;

}

/*
	Filtra le posizioni del robot rimuovendo le imprecisioni della kinect
	e garantendo la stabilità del robot

	Params:
      const vector<double> & positions, 
      vector<double> & filtered_positions
*/
void filter(const vector<double> & positions, vector<double> & filtered_positions) {
	
	cout << "\nFiltering positions..." << endl;

	filtered_positions = positions;

	keepBestLeg(filtered_positions); // l'info nell'history è mediamente sbagliata
									 // meglio non metterci i valori sicuramente sballati

	keepBestArm(filtered_positions, arms_mirroring); // l'info nell'history è mediamente sbagliata
									 				 // meglio non metterci i valori sicuramente sballati
	
	storeInHistory(filtered_positions);
	
	filtered_positions = getHistoryAvgs();

	keepBestLeg(filtered_positions);

	keepBestArm(filtered_positions, arms_mirroring);

	filtered_positions[right_knee_joint_pitch] = estimate_knee_pitch(filtered_positions);
	filtered_positions[right_ankle_joint_pitch] = estimate_ankle_pitch(filtered_positions);

	removeCollisions(filtered_positions, arms_mirroring);

	filtered_positions[right_hip_joint_pitch] += -0.09; // per tenerlo leggermente in avanti

	right2leftMirror(filtered_positions, arms_mirroring);

}

/*
	send_final_positions invia al robot le posizioni filtrate,
	l'invio standard invia le filtrate così come sono,
	l'invio con split invia una media (eventualmente pesata) tra posizione iniziale e finale
	per evitare che il robot si muova di scatto verso la finale

	Params:
      const sensor_msgs::JointState::ConstPtr & msg, // posizioni iniziali
	  const vector<double> & filtered_positions, // posizioni finali
	  const bool split = false
*/
void send_final_positions(const sensor_msgs::JointState::ConstPtr & msg, // posizioni iniziali
						  const vector<double> & filtered_positions, // posizioni finali
						  const bool split = false)
{
  sensor_msgs::JointState filtered_msg;
  filtered_msg = *msg;
  filtered_msg.position = filtered_positions;

  // invio in più colpi
  // attenzione ai segni
  // per evitare complicazioni meglio fare la media (eventualmente pesata) tra iniziale e finale
  // non serve fare due publish, il secondo (il terzo, etc... se media pesata) è fatto al giro dopo
  if (split)
  {
  	cout << "posizione finale raggiunta incrementalmente" << endl;
  	// l'iniziale non può essere usata così com'è perché ad esempio la caviglia è sempre a 0
  	double hip_ankle_reference = -abs(msg->position[right_hip_joint_pitch]);
  	double knee_reference = abs(hip2knee(hip_ankle_reference));
  	double ti_weight = 2.0;
  	double tf_weight = 1.0;
  	double norm_term =  ti_weight + tf_weight;

  	// (posa iniziale + posa finale) / 2.0
    filtered_msg.position[right_hip_joint_pitch] = (ti_weight*hip_ankle_reference + tf_weight*filtered_positions[right_hip_joint_pitch]) / norm_term;
    filtered_msg.position[left_hip_joint_pitch] = (ti_weight*hip_ankle_reference + tf_weight*filtered_positions[left_hip_joint_pitch]) / norm_term;
    filtered_msg.position[right_knee_joint_pitch] = (ti_weight*knee_reference + tf_weight*filtered_positions[right_knee_joint_pitch]) / norm_term;
    filtered_msg.position[left_knee_joint_pitch] = (ti_weight*knee_reference + tf_weight*filtered_positions[left_knee_joint_pitch]) / norm_term;
    filtered_msg.position[right_ankle_joint_pitch] = (ti_weight*hip_ankle_reference  + tf_weight*filtered_positions[right_ankle_joint_pitch]) / norm_term;
    filtered_msg.position[left_ankle_joint_pitch] = (ti_weight*hip_ankle_reference + tf_weight*filtered_positions[left_ankle_joint_pitch]) / norm_term;
    //pub.publish(filtered_msg);
  }

  // invio standard in un colpo
  pub.publish(filtered_msg);
}

/*
	This callback filters the joints_states requested by the kinect system and forwards them
	to the robovie_x_controller which actuates the requested movements

	Params:
	  const sensor_msgs::JointState::ConstPtr& msg
*/
void callbackJointState(const sensor_msgs::JointState::ConstPtr& msg) {

	vector<double> positions = msg->position;

    vector<double> filtered_positions;
	filter(positions, filtered_positions);

	send_final_positions(msg, filtered_positions);
}

/*
	Print out gyro info

	Params:
	  const sensor_msgs::Imu::ConstPtr& msg
*/
void callbackGyro(const sensor_msgs::Imu::ConstPtr& msg) {

	// DEBUGGING output!
	cout << "\n\nImu data:" << endl;
	cout << "\tframe id " << msg->header.frame_id << endl;
	cout << "\torientation x " << msg->orientation.x << endl;
	cout << "\torientation y " << msg->orientation.y << endl;
	cout << "\torientation z " << msg->orientation.z << endl;
	cout << "\torientation w " << msg->orientation.w << endl;
	cout << "\tvelocity x " << msg->angular_velocity.x << endl;
	cout << "\tvelocity y " << msg->angular_velocity.y << endl;
	gyro = *msg;

}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "filtering");
	ros::NodeHandle nh;
	
	ros::Subscriber sub_joint_states = nh.subscribe("joint_states_raw", 10, callbackJointState);
	//ros::Subscriber sub2_imu = nh.subscribe("imu", 10, callbackGyro);
	pub = nh.advertise<sensor_msgs::JointState>("joint_states", 200); // read by robovie_x_controller!

	angle_history = vector<vector<double> >();
	for(int i = 0; i < 17; i++) {
		angle_history.push_back(vector<double>(history_size));
	}

	ros::Rate rate(10);

	//CoMUtilities com(0.0355, 0.09, true);

	while(ros::ok())
	{
		//com.calculate_com();
		
		ros::spinOnce();
		rate.sleep();
	}

	return 0;
}